#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  HTMotor)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     IR1,            sensorI2CCustomFastSkipStates)
#pragma config(Sensor, S3,     IR2,            sensorI2CCustomFastSkipStates)
#pragma config(Motor,  mtr_S1_C1_1,     L,             tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     R,             tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     motorCR,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_2,     motorCL,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C4_1,     motorBall,     tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C4_2,     motorI,        tmotorNone, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    servo1,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_3,    Bridge,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_5,    Lift,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    Lift2,                tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                           Autonomous Mode Code Template
//
// This file contains a template for simplified creation of an autonomous program for an TETRIX robot
// competition.
//
// You need to customize two functions with code unique to your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    initializeRobot
//
// Prior to the start of autonomous mode, you may want to perform some initialization on your robot.
// Things that might be performed during initialization include:
//   1. Move motors and servos to a preset position.
//   2. Some sensor types take a short while to reach stable values during which time it is best that
//      robot is not moving. For example, gyro sensor needs a few seconds to obtain the background
//      "bias" value.
//
// In many cases, you may not have to add any code to this function and it will remain "empty".
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

void initializeRobot()
{
  servo[Lift] = 8;
  servo[Bridge] = 240;
  servo[Lift2] = 248;
  return;
}


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                         Main Task
//
// The following is the main code for the autonomous robot operation. Customize as appropriate for
// your specific robot.
//
// The types of things you might do during the autonomous phase (for the 2008-9 FTC competition)
// are:
//
//   1. Have the robot follow a line on the game field until it reaches one of the puck storage
//      areas.
//   2. Load pucks into the robot from the storage bin.
//   3. Stop the robot and wait for autonomous phase to end.
//
// This simple template does nothing except play a periodic tone every few seconds.
//
// At the end of the autonomous period, the FMS will autonmatically abort (stop) execution of the program.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

 #include "drivers\HTIRS2-driver.h"

#ifndef max
#define max( a, b ) ( ((a) > (b)) ? (a) : (b) )
#endif

float currDir;
float IRGetACDir(tSensors link)
{
  static float prevDir = 0.0;

  int acS[5];
  int idx;
  idx = HTIRS2readACDir(link);
  currDir = (float)idx;
  if (idx == 0)
  {
    currDir = prevDir;
  }
  else if (HTIRS2readAllACStrength(link, acS[0], acS[1], acS[2], acS[3], acS[4]))
  {
    idx = (idx - 1)/2;
    if ((idx < 4) && (acS[idx] != 0) && (acS[idx + 1] != 0))
    {
      currDir += (float)(acS[idx + 1] - acS[idx])/
      max(acS[idx], acS[idx + 1]);
    }
    nxtDisplayTextLine(0, "Idx=%d,Dir=%5.1f", idx, currDir);
    nxtDisplayTextLine(2, "S1=%d,S2=%d", acS[0], acS[1]);
    nxtDisplayTextLine(3, "S3=%d,S4=%d", acS[2], acS[3]);
    nxtDisplayTextLine(4, "S5=%d", acS[4]);
  }
  prevDir = currDir;

  return currDir;
}

void irmove()
{
  float Dir=0;
  ClearTimer(T1);
  while(time100[T1]<115)
{
    Dir=IRGetACDir(IR1)-(10-IRGetACDir(IR2));
    nxtDisplayBigTextLine(5,"%f",Dir);
    if (Dir<0)
    {
      motor[R]=65;
      motor[L]=-65;
    }
    if (Dir>0)
    {
      motor[R]=-60;
      motor[L]=60;
    }
    if (Dir==0)
    {
      motor[R]=-50;
      motor[L]=-50;

    }
  }
}



task main()
{
  initializeRobot();

  waitForStart(); // Wait for the beginning of autonomous phase.


motor[L] = 85;
motor[R] = 85;
wait1Msec(2200);


irmove();



}
